1.- Definiciones iniciales

En primer lugar, se importan todas las librerías que se utilizarán.

A continuación, se declaran algunas funciones que serán útiles a la hora de mostrar los resultados.

La función join_plot realiza gráficas de manera conjunta de los datos contenidos en el parámetro array_plot. Este parámetro es una lista de arrays, donde las columnas de cada array serán ploteadas en gráficas distinta. Si la lista tiene más de un array, las gráficas de los elementos de la lista serán supuerpuestas unas con otras.

RMSE: Root Mean Square Error (Error cuadrático medio)

La siguiente función se utiliza para calcular el RMSE entre dos conjuntos de vectores.

rmse

Este indicador es útil para comparar errores entre mediciones con igual unidad. En la implementación se utiliza la función mean_squared_error de la librería sklearn.metrics. Por defecto, el parámetro squared esta seteado en True y la función rentorna el MSE. Si se lo establece en false, el resultado es el RMSE.

2.- Implementación del filtro de Kalman

La función kalman implementa el filtro de Kalman sobre los datos medidos en los parámetros noise_pos y noise_vel. Las matrices A, C, Q y R son pasadas como parámetros al filtro, asi como también las mediciones iniciales x0 y el estado inicial de la matriz de covariancia de medición P0.

La función de desarrolla dentro de un ciclo for, en donde se procesa cada medición. En primer lugar, en cada instante de tiempo, se define el vector de mediciones Z. Si el vector noise_vel esta vacío, sólo se toman mediciones de posición, por lo que el vector de mediciones Z tendrá la forma: Z = [pos_x, pos_y, pos_z]. Si se añaden mediciones de velocidad, el vector de mediciones es: Z = [pos_x, vel_x, pos_y, vel_y, pos_z, vel_z].

Luego, el filtro opera en dos ciclos. El primero es el ciclo de predicción, que utiliza la matriz del sistema A y la covariancia del ruido Q. El segundo es el ciclo de actualización de medición, que utiliza la ganancia de kalman K y el error en la predicción en el instante de tiempo anterior.

Finalmente, los valores estimados se almacenan en vectores y son devueltos por la funcion.

Algunas de las matrices que se utilizan en el filtro de Kalman dependen sólo del modelo del sistema, o de las características de los sensores utilizados. Estas matrices no serán modificadas a lo largo del trabajo y es por eso que podemos definirlas en esta sección de manera global.

3.- Carga de datos

Los datos son cargados utilizando al librería "Pandas", de los archivos posicion.dat, velocidad.dat y aceleracion.dat. Luego se transforman en objetos "numpy" para facilitar su operación.

4.- Generación de mediciones con ruido

En esta sección se crean las mediciones con ruido superpuesto a la posición y velocidad

4.1.- Agregado de ruido gaussiano a la posición

Mediante el uso de la función random.normal se genera un conjunto de datos con distribución gaussiana, de media 0 y desviación estandar 10. Los datos se almacenan en una matriz con las mismas dimensiones que el vector de posiciones cargado previamente (es decir 3x351). Luego, se muestra el histograma de la distribución del ruido.

Finalmente, el ruido es añadido a la matriz de datos de posiciones suamando ambas matrices. Se grafica el resultado para cada eje, con zoom en el rango de tiempo [10,60].

4.2.- Agregado de ruido uniforme a la posición

En este caso, se utiliza la función random.uniform para generar los vectores de ruido uniforme. Los parámetros que recibe esta función son los extremos [a,b] del intervalo. La variancia en la distribución uniforme, viene dada por: uniform

Por lo tanto, sabiendo el valor de la variancia podemos despejar el ancho del intervalo [a,b]. Luego, se dibuja el histograma y se agrega el ruido a la matriz de datos de posiciones.

4.3.- Agregado de ruido gaussiano a la velocidad

De manera similar al punto 4.1 se genera una matriz de ruido gaussiano y se agrega a las mediciones de velocidad.

5.- Aplicación del filtro de Kalman

5.1.- Estimaciones con medición de posición con ruido gaussiano

Antes de generar las estimaciones se deben definir las matrices C y R, que dependen de cada caso en particular.

R es la matriz de covarianza del ruido y representa la incertidumbre en las mediciones de posición. Dado que el ruido en cada eje no está correlacionado con los demás, los términos de covariancias son nulos y R adquiere una estructura diagonal. Los valores de la diagonal son las variancias del ruido en cada eje (en este caso 100).

Por su parte, C es la matriz de obsevación. Es una matriz formada principalmente por ceros, donde los unos nos indican que valores están siendo medidos. En este caso, las mediciones observadas serán las posiciones en cada eje de coordenadas. Tener en cuenta que el vector de estados tiene la forma:

x0 = [pos_x, vel_x, ace_x, pos_y, vel_y, ace_y, pos_z, vel_z, ace_z]

Finalmente, las matrices y las mediciones son pasadas como parámetro a la función kalman() definida previamente, que retorna las predicciones de posición, velocidad y aceleración. Además, las estimaciones son graficadas y contrastadas con los valores reales

5.2.- Estimaciones con medición de posición con ruido uniforme

Se repiten los pasos realizados en el apartado anterior. Dado que las mediciones y el vector de estados son idénticos al apartado anterior, la matriz de observación C no se modifica. De igual forma, si bien el ruido tiene otra distribución, su variancia sigue manteniendo el mismo valor, por lo que la matriz R también coincide con la definida previamente.

5.3.- Estimaciones con mediciones de posición y velocidad con ruido gaussiano

En este caso, la matriz C se debe modifcar para agregar una nueva medición a las observaciones. Además, se agregan a R los términos correspondientes a la variancia del ruido en las velocidades.

6.- Comparación de resultados

A continuación se grafica la evolución de los errores de posición, velocidad y aceleración a lo largo del tiempo para cada caso analizado. Este error es calculado como el valor absoluto de la diferencia entre el valor real de la variable y el valor estimado por el filtro:

mape

Podemos ver que en general, el error arranca en un valor relativamente alto pero rápidamente decrese y oscila entre valores menores. En la mayoría de los casos, podemos considerar que a partir de t=30s el error ya llegó a sus valores finales.

Por otro lado, si observamos las gráficas de los dos primeros casos, se puede ver que el comportamiento de las cruvas de error son similares y oscilan etre aproximádamente los mismos valores. Es decir que los errores obtenidos en las estimaciones a partir de mediciones con ruido de distribución uniforme o gaussiana, son similares.

Sin embargo, al agregar la medición de velocidad los resultados mejoran notablemente, no solo en los valores de velocidad y aceleración, sino también en los de posición.

A continuación se calcula el error cuadrático medio (RMSE) de cada gráfica para tener una visión de los resultados de manera global.

Vemos que los resultados se condicen con lo comentado anteriormente. El RMSE para el caso de medición de posición con ruido gaussiano, es prácticamente igual al de medición con ruido uniforme. Cabe destacar que, si bien el error en las estimaciones de velocidad y aceleración para los dos primeros casos es alto, estamos obteniendo dos variables nuevas a partir de la medición de una tercera. Es decir que, a pesar de tener un error alto, se logra conocer los valores de variables que no podemos medir.

Al agregar la medición de velocidad, los resultados mejoran para todas las variables analizadas. En la siguiente celda, puede verse el porcentaje de mejora para cada varable.

Nota: Si bien los valores absolutos de RMSE para al aceleración son menores que para la posición, hay que tener en cuenta que son relativos a la magnitud medida.

Finalmente, se grafica la posición medida con ruido gaussiano, superpueta por la estmada por el filtro, tomando como entrada medición de posición y velocidad. Puede verse en estas gráficas, que la salida del filtro logra eliminar en gran medida el ruido de la medición.

7.- Conclusiones

En este trabajo desarrollado, se ha desarrollado una aplicación básica del filtro de kalman. Por un lado, el filtro permite predecir la posición y velocida de un objeto, a partir de la medición de su posición. Esta es una característica muy últil ya que no siempre es posible medir la velocidad y aceleración de los objetos.

Por otro lado, se comprobó que mediante su uso se puede obtener una mejor estimación de las variables, que las mediciones directas tomada de los sesores. Además, permite integrar las mediciones de distintas variables, como posición y velocidad, y obtener como resultado una mejor estimación de ambas.

Finalmente, se comprobó que los resultados del filtro no dependen de la distribución del ruido en la medición. Sin embargo, las estmiaciones mejoran notablemente cuando se agregan más mediciones.